import sensor, lcd
import time
from modules import ybkey
from modules import ybrgb

RGB = ybrgb()
KEY = ybkey()
RGB.set(1, 0, 0)
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)   # QVGA:320x240 - QQVGA:160x120

sensor.set_auto_exposure(1)                                 # 设置自动曝光
#sensor.set_auto_exposure(0, exposure=120000)               # 设置手动曝光 曝光时间 120000 us
sensor.set_auto_gain(0, gain_db = 17)                       # 设置画面增益 17 dB 影响实时画面亮度
sensor.set_auto_whitebal(0, rgb_gain_db = (0,0,0))          # 设置RGB增益 0 0 0 dB 影响画面色彩呈现效果 在 K210 上无法调节增益 初步判定是感光元件 ov2640 无法支持
sensor.run(1)
sensor.skip_frames(time = 2000)

clock = time.clock()
#*********************************************
red_thresholds = [(81, 100, 2, 127, -128, 127)]
green_thresholds = [(81, 100, -41, -15, -128, 127)]
roi_ = [49,30,226,161]
rect_ = 0
red_point_ = 0
green_point_ = 0
#*********************************************

# 找矩形
def find_rect(img):
    global rect_
    magnitude = 0
    for r in img.find_rects(roi=roi_,threshold=50000):
        # 查找最大面积值
        if r.magnitude() > magnitude:
            rect_ = r

# 找光点
def _find_point_(img,threshold_):
    point_ = 0
    area = 0
    for r in img.find_blobs(threshold_,roi=roi_):
        if r.area() > area:
            area = r.area()
            point_ = r
    return point_

def find_point(img):
    global red_point_,green_point_
    red_point_ = _find_point_(img,red_thresholds)
    green_point_ = _find_point_(img,green_thresholds)

# 画矩形画圆点
def draw(img):
    # 画矩形
    if rect_ != 0:
        img.draw_rectangle(rect_.rect(), color = (255, 0, 0), thickness=1)
        for p in rect_.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)
    # 画圆点
    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if red_point_ != 0:
        img.draw_circle(red_point_.cx(), red_point_.cy(), 5, thickness=3, color = (0, 255, 0))
        img.draw_string (red_point_.cx(), red_point_.cy()+5, str(red_point_.cx())+','+str(red_point_.cy()), color=(255, 255, 255),mono_space=False)

    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if green_point_ != 0:
        img.draw_circle(green_point_.cx(), green_point_.cy(), 5, thickness=3, color = (255, 0, 0))
        img.draw_string (green_point_.cx(), green_point_.cy()+5, str(green_point_.cx())+','+str(green_point_.cy()), color=(255, 255, 255),mono_space=False)
# 按键检测更新ROI区域
def key_click():
    global rect_
    if KEY.is_press() == 1:
        RGB.set(0, 1, 0)
        size = 20
        if rect_ != 0:
            roi_[0] = rect_.x() - size
            roi_[1] = rect_.y() - size
            roi_[2] = rect_.w() + size * 2
            roi_[3] = rect_.h() + size * 2
        else :  # 如果没有找到矩形就将ROI缩小
            RGB.set(0, 0, 1)
            roi_[0] = roi_[0] + 2
            roi_[1] = roi_[1] + 2
            roi_[2] = roi_[2] - 4
            roi_[3] = roi_[3] - 4
while(True):
    clock.tick()
    img = sensor.snapshot()
    fps = clock.fps()

    key_click()
    find_rect(img)
    find_point(img)
    draw(img)
    img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 0, 255), scale=1.0)
    lcd.display(img)
    print(fps)


